#include <iomanip>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <sstream>
#include <string>
#include <vector>

int main()
{
    // 棋盘格规格（内角点数）
    const int board_width = 24;
    const int board_height = 15;
    const float square_size = 30.0f; // 单位为 mm

    cv::Size board_size(board_width, board_height);
    std::vector<std::vector<cv::Point3f>> object_points;
    std::vector<std::vector<cv::Point2f>> image_points;

    std::vector<cv::Point3f> objp;
    for (int i = 0; i < board_height; i++)
    {
        for (int j = 0; j < board_width; j++)
        {
            objp.emplace_back(j * square_size, i * square_size, 0);
        }
    }

    // 图像路径前缀
    std::string base_path = "/home/linux/Desktop/opencv/camera_calibration/chessboard_images/";
    int valid_images = 0;

    for (int i = 1; i <= 10; ++i)
    {
        std::ostringstream ss;
        ss << base_path << i << ".png";
        cv::Mat image = cv::imread(ss.str());
        if (image.empty())
        {
            std::cerr << "无法读取图像: " << ss.str() << std::endl;
            continue;
        }

        cv::Mat gray;
        cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);

        std::vector<cv::Point2f> corners;
        bool found = cv::findChessboardCorners(gray, board_size, corners,
                                               cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);

        if (found)
        {
            cv::cornerSubPix(gray, corners, cv::Size(11, 11), cv::Size(-1, -1),
                             cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.001));
            image_points.push_back(corners);
            object_points.push_back(objp);
            valid_images++;

            cv::drawChessboardCorners(image, board_size, corners, found);
            cv::imshow("Corners", image);
            cv::waitKey(100); // 显示100ms
        }
        else
        {
            std::cerr << "未找到角点: " << ss.str() << std::endl;
        }
    }

    if (valid_images < 1)
    {
        std::cerr << "没有足够的有效图像用于标定。" << std::endl;
        return -1;
    }

    // 标定相机
    cv::Mat cameraMatrix, distCoeffs, R, T;
    std::vector<cv::Mat> rvecs, tvecs;
    double rms = cv::calibrateCamera(object_points, image_points, cv::Size(640, 480),
                                     cameraMatrix, distCoeffs, rvecs, tvecs);

    std::cout << "标定完成，重投影误差 RMS: " << rms << std::endl;
    std::cout << "相机内参矩阵:\n"
              << cameraMatrix << std::endl;
    std::cout << "畸变系数:\n"
              << distCoeffs << std::endl;

    cv::destroyAllWindows();
    return 0;
}
